#include "usr_inc.h"

void StartBattleCmdDeal(void *argument)
{
    for (;;)
    {
        BattleCmdMsg_t battleCmd;
        xQueueReceive(battleCmdQueueHandle, &battleCmd, portMAX_DELAY);

        if (battleCmd == BATTLE_PREPARE)
        {
            TossCmdMsg_t tossCmd;
            DribbleCmdMsg_t dribbleCmd;

            /* 拉模式 */
            tossCmd = TOSS_DRAG;
            xQueueSendToBack(tossCmdQueueHandle, &tossCmd, 1);
            vTaskDelay(10);

            /* 拉一段距离, 用于运球后的接球 */
            float stroke = 0.50;
            xQueueSendToBack(tossStrokeQueueHandle, &stroke, 1);

            /* 伸 */
            dribbleCmd = DRIBBLE_EXTEND;
            xQueueSendToBack(dribbleCmdQueueHandle, &dribbleCmd, 1);
            vTaskDelay(800);

            /* 吸 */
            dribbleCmd = DRIBBLE_DRAW;
            xQueueSendToBack(dribbleCmdQueueHandle, &dribbleCmd, 1);
        }
        else if (battleCmd == BATTLE_GO)
        {
            DribbleCmdMsg_t dribbleCmd;

            /* 稳 */
            dribbleCmd = DRIBBLE_HOLD;
            xQueueSendToBack(dribbleCmdQueueHandle, &dribbleCmd, 1);
            vTaskDelay(200);

            /* 连续运球两次 */
            dribbleCmd = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &dribbleCmd, 1);
            vTaskDelay(1100);
            dribbleCmd = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &dribbleCmd, 1);
            vTaskDelay(800);

            /* 回 */
            dribbleCmd = DRIBBLE_RETRACT;
            xQueueSendToBack(dribbleCmdQueueHandle, &dribbleCmd, 1);
            vTaskDelay(450);

            /* 落 */
            dribbleCmd = DRIBBLE_FREE;
            xQueueSendToBack(dribbleCmdQueueHandle, &dribbleCmd, 1);
            vTaskDelay(400);
        }
        else if (battleCmd == BATTLE_SHOOT)
        {
            TossCmdMsg_t tossCmd;
            AimMode_t aimMode;

            /* 放 */
            tossCmd = TOSS_FREE;
            xQueueSendToBack(tossCmdQueueHandle, &tossCmd, 1);
            vTaskDelay(1500);

            /* 不对准 */
            aimMode = AIM_DISABLE;
            xQueueSendToBack(aimModeQueueHandle, &aimMode, 2);
        }
        else if (battleCmd == BATTLE_RESET)
        {
            DribbleCmdMsg_t dribbleCmd;
            TossCmdMsg_t tossCmd;
            AimMode_t aimMode;
            ChassisCmdMsg_t chassisCmd;

            /* 回 */
            dribbleCmd = DRIBBLE_RETRACT;
            xQueueSendToBack(dribbleCmdQueueHandle, &dribbleCmd, 1);

            /* 落 */
            dribbleCmd = DRIBBLE_FREE;
            xQueueSendToBack(dribbleCmdQueueHandle, &dribbleCmd, 1);

            /* 不放 */
            tossCmd = TOSS_ABANDON;
            xQueueSendToBack(tossCmdQueueHandle, &tossCmd, 1);

            /* 不对准 */
            aimMode = AIM_DISABLE;
            xQueueSendToBack(aimModeQueueHandle, &aimMode, 2);

            /* 设置为锁头模式 */
            chassisCmd = CHASSIS_CONF_YAW;
            xQueueSendToBack(chassisCmdQueueHandle, &chassisCmd, 1);
        }
    }
}
